#ifndef STRUCTTASKSTATE_H
#define STRUCTTASKSTATE_H



#include "FreeRTOS.h"
#include <p32xxxx.h>
#include "types.h"
#include "driverLog.h"

#define MAGN_BUFFERSIZE				16
#define MPU_GYRO_BUFFERSIZE			16
#define MPU_ACC_BUFFERSIZE			16
#define BARO_BUFFERSIZE				16


#define RC_AXIS_THROTTLE			0
#define RC_AXIS_AILERON				1
#define RC_AXIS_ELEVATOR			2
#define RC_AXIS_RUDDER				3
#define RC_AXIS_GEAR				4
#define RC_AXIS_AUX1				5
#define RC_AXIS_AUX2				6
#define RC_AXIS_AUX3				7

#define EDITMODE_OFF				0
#define EDITMODE_SELECT_PID			1
#define EDITMODE_SELECT_VAR			2

#define EDITMODE_VAR_P				0
#define EDITMODE_VAR_I				1
#define EDITMODE_VAR_D				2
#define EDITMODE_VAR_ILIM			3
#define EDITMODE_VAR_LIM			4
#define EDITMODE_VAR_RANGE			5

typedef enum 
{
    MOTOR_MODE_OFF,
    MOTOR_MODE_FLIGHT,
    MOTOR_MODE_DEBUG,
    MOTOR_MODE_BODE
} MOTOR_MODE;

struct imuOutputStruct_t
{
	q_t pitch;
	q_t roll;
	q_t yaw;

	q_t pitchD;
	q_t rollD;
	q_t yawD;

	q_t zSpeed;
	q_t zPosition;
};

struct PidState_t {
	BYTE calculated;

	q_t Isum;
	q_t prevError;
	q_t prevSetPoint;

    struct
    {
        q_t setpoint;
        q_t feedback;
    	q_t output;
    } values;
};


/*typedef struct {
	unsigned imuFix:1;
	unsigned logFile:1;
	unsigned logWireless:1;
	unsigned quadBooting:4;
	unsigned quadConfig:1;
	unsigned engines:1;
	unsigned rfConnected:1;
} _quadState;*/

struct StateStruct_t
{
    struct PidState_t pid[16];
    
    struct {
        unsigned int quadTaskTickCount;
        unsigned int stabilityTaskTickCount;
        unsigned int driverRN41TickCount;
        unsigned int driverSDTickCount;
        q_t vLipo;
        q_t current;
    } monitoring;

	struct {
		unsigned int failures;
		unsigned int totalSamples;

		SHORT3 buffer[MAGN_BUFFERSIZE];
		BYTE bufferPosition;

		unsigned connected:1;
		unsigned bufferFilled:1;
		unsigned calibrated:1;
	} hmc5883;

	struct {
		unsigned int failures;
		unsigned int totalSamples;

		SHORT3 gyroBias;
		
		SHORT3 bufferGyro[MPU_GYRO_BUFFERSIZE];
		SHORT3 bufferAcc[MPU_GYRO_BUFFERSIZE];
		BYTE bufferGyroPosition;
		BYTE bufferAccPosition;

        unsigned connected:1;
		unsigned accBufferFilled:1;
		unsigned gyroBufferFilled:1;
		unsigned gyroBiassed:1;
		unsigned accMatrixSet:1;
	} mpu6050;

	struct {
		unsigned int failures;
		unsigned int totalSamples;

		q_t buffer[BARO_BUFFERSIZE];
		BYTE bufferPosition;

		unsigned connected:1;
		unsigned bufferFilled:1;
		unsigned calibrated:1;
	} mpl3115;

	struct {
		unsigned short seconds;
		q_t vLipo;
		q_t totalCurrent;

        unsigned int uniqueId;
        unsigned configLoaded:1;

        unsigned int unusedHeap;
	} system;

    struct {
        unsigned active:1;

        unsigned int sizeKBytes;
        unsigned int sizeSectors;
        //unsigned int sizeFreeSectors;
        unsigned int sizeWrittenSectors;
        
        unsigned char cardType;
        // 41464 / 70008
        unsigned char cardV1;
        unsigned char cardHighCapacity;

        unsigned char fatDetected;

        unsigned char currentFATLocation; // used for wear leveling
        unsigned char currentConfigLocation; // ...
    } sd;

    struct {
        unsigned loggingEnabled:1;
        unsigned manualLoggingEnabled:1;
        unsigned manualLoggingDisabled:1;
        unsigned int shouldUpdateFAT;

        unsigned int fatEntryId; // FAT entry
        unsigned int logBlock; // Location of block to write to
        unsigned int freeBlocks;
    } sdLogging;

    struct {
        unsigned char busy;
        unsigned int amplitude;
        unsigned int frequency; // * 10
        unsigned int controlFrequency;
        unsigned int duration;
        unsigned int durationCount;
        unsigned int step;
        unsigned int stepReceived;
        int value;

        /*q_t Re_cos_Error;
        q_t Im_sin_Error;
        q_t Re_cos_Out;
        q_t Im_sin_Out;
        q_t Re_cos_In;
        q_t Im_sin_In;
        q_t Re_cos_Signal;
        q_t Im_sin_Signal;*/
        float Re_cos_Error;
        float Im_sin_Error;
        float Re_cos_Out;
        float Im_sin_Out;
        float Re_cos_In;
        float Im_sin_In;
        float Re_cos_Signal;
        float Im_sin_Signal;
    } bode;

    struct {
        MOTOR_MODE mode;

        unsigned char count;
        unsigned char address[8];
        unsigned char in_bootloader[8];

        unsigned char debugSelected;

        struct {
            short setPointRPM;
            short feedbackRPM;
        } setpoints[8];

        struct {
            short setpoint;
            short feedback[3];
        } tuning;

        struct {
            short error;
            short pid;
            short signal;
        } bode;

        unsigned char bodeFeedback;
    } motors;

	struct {
		BYTE mode;

		// calculated from config.sensor.accGravity
		q_matrix_t	accGravityMatrix;

		// length of config.sensor.accGravity
		q_t           accGravityNormalize;

		q_vector_t accFiltered;		// filtered vector of raw accelerometer
		q_vector_t accFilteredTransposed;	// transposed to initial gravity
		q_vector_t accFilteredScaled; // and then scaled to gravity
		q_vector_t accFilteredNormalised; // and normalised

		q_vector_t accRotated; // rotated to pitch/roll

		q_vector_t gyroFiltered;
		q_vector_t gyroScaledToSec;
		q_vector_t gyroAutoBias;

		q_t baroFiltered;
		q_t baroBase;

		q_vector_t magnBase;
		q_t magnBaseLength;
		q_vector_t magnFiltered;


		struct imuOutputStruct_t output;
	} imu;

	struct {
		BYTE mode;

		q_t targetPitch;
		q_t targetRoll;
		q_t targetYawD;

		q_t baseEngines;
	} stabilize;

    struct {
        SHORT varEnabled[LOG_ID_MAX_VALUE];
        TickType_t varLastSent[LOG_ID_MAX_VALUE];

        BYTE disableSending;
    } btLogging;

	struct {
		SHORT axis[8];
        q_t     axisQ[8];
		SHORT oldAxis[8];
        BYTE channelChanged;

		TickType_t lastReceived;
	} rc;
};

extern struct StateStruct_t _state;
#define STATE (_state)

#endif
